// -----------------------------------------------------------------------------
// Copyright (c) 2024 Mu Shibo
// Modify by Tommy 2025.8.11
// -----------------------------------------------------------------------------

#include <esp_now.h>
#include <WiFi.h>
#include "main.h"

/************实例定义*************/

// 电机实例
BLDCMotor motor1 = BLDCMotor(7);
BLDCMotor motor2 = BLDCMotor(7);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(32, 33, 25, 22);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(26, 27, 14, 12);

// 编码器实例
TwoWire I2Cone = TwoWire(0);
TwoWire I2Ctwo = TwoWire(1);
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C);

// PID控制器实例
PIDController pid_angle(1, 0, 0, 100000, 8); // pitch角度PID使其趋于零点
PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点
PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点
PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度
PIDController pid_yaw_angle(1.0, 0, 0, 100000, 8);
PIDController pid_yaw_gyro(0.04, 0, 0, 100000, 8);
PIDController pid_lqr_u(1, 15, 0, 100000, 8);
PIDController pid_zeropoint(0.002, 0, 0, 100000, 4);
PIDController pid_roll_angle(8, 0, 0, 100000, 450);

// 低通滤波器实例
LowPassFilter lpf_joyy(0.2);
LowPassFilter lpf_zeropoint(0.1);
LowPassFilter lpf_roll(0.3);

// WebServer实例
WebServer webserver;                               // server服务器
WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
RobotProtocol rp(20);

// STS舵机实例
SMS_STS sms_sts;

// MPU6050实例
MPU6050 mpu6050(I2Ctwo);

/************参数定义*************/
#define pi 3.1415927

// 舵机位置限制参数
#define SERVO1_MIN 1900 // 舵机1最低 从小车正面看右手
#define SERVO1_MAX 2400 // 舵机1最高
#define SERVO2_MIN 2300 // 舵机2最低 从小车正面看左手(注意大小值与舵机1是相反的)
#define SERVO2_MAX 1800 // 舵机2最高

#define  LEG_HEIGHT_BASE 32  // 腿部默认高度基准值

// LQR自平衡控制器参数
float LQR_angle = 0;
float LQR_gyro = 0;
float LQR_speed = 0;
float LQR_distance = 0;
float angle_control = 0;
float gyro_control = 0;
float speed_control = 0;
float distance_control = 0;
float LQR_u = 0;
float angle_zeropoint = 5;  // 设定默认俯仰角度，向前到正，向后倒负
float distance_zeropoint = 1; // 轮部位移零点偏置

// YAW轴控制数据
float YAW_gyro = 0;
float YAW_angle = 0;
float YAW_angle_last = 0;
float YAW_angle_total = 0;
float YAW_angle_zero_point = -10;
float YAW_output = 0;


// 腿部舵机控制数据
byte ID[2];
s16 Position[2];
u16 Speed[2];
byte ACC[2];

// 逻辑处理标志位
float robot_speed = 0;         // 记录当前轮部转速
float robot_speed_last = 0;    // 记录上一时刻的轮部转速
int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识
int jump_flag = 0;             // 跳跃时段标识
float leg_position_add = 0;    // roll轴平衡控制量

// 电压检测
uint16_t bat_check_num = 0;
int BAT_PIN = 35; // select the input pin for the ADC
static esp_adc_cal_characteristics_t adc_chars;
static const adc1_channel_t channel = ADC1_CHANNEL_7;
static const adc_bits_width_t width = ADC_WIDTH_BIT_12;
static const adc_atten_t atten = ADC_ATTEN_DB_11;
static const adc_unit_t unit = ADC_UNIT_1;

int loopCounter = 0; // loop函数执行计数
int resetToDefaultCounter = 0; // resetToDefault执行计数

// 电量显示LED
#define LED_BAT 13

// 定义摇杆数值死区参数
const int deadZone = 15;

typedef struct {
  byte j1PotX;  // 左摇杆X  (0-255)
  byte j1PotY;  // 左摇杆Y  (0-255)
  byte j2PotX;  // 右摇杆X  (0-255)
  byte j2PotY;  // 右摇杆Y (0-255)
  bool buttonR1;  //底部右按键 1：true （默认），0：false
  bool buttonR2;  //底部左按键 1：true （默认），0：false
  bool j1Button;  //左摇杆按键 1：true （默认），0：false
  bool j2Button;  //右摇杆按键 1：true （默认），0：false
  bool buttonLB;  //顶部左按键 1：true （默认），0：false
  bool buttonRB;  //顶部右按键 1：true （默认），0：false
  bool tSwitch1;  //拨动开关   1：true （默认），0：false 
  byte roll;      //用于M5StackCore2或未来增加的陀螺仪功能
  byte pitch;
  bool buttonR3;
} JoystickData;

JoystickData remoteData;

// 蓝灯爆闪
bool lastbuttonLB = false;

// ESP-NOW接收回调函数
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
  memcpy(&remoteData, incomingData, sizeof(remoteData));

  // 跳跃状态不接受指令
  if(jump_flag > 0){
    wrobot.dir = 4; // 不跳跃
    wrobot.joyx = 0;
    wrobot.joyy = 0;
    wrobot.roll = 0;
    return;
  }

  // 摇杆输出范围0-255，转换到 -100 到 100
  // roll 左右摇摆
  int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30
  // 上下高度
  int convertedJ1PotY = static_cast<int>((1 - remoteData.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0  y轴取反，这样可以使机身下降
  // 左右旋转
  int convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100
  // 前后速度
  int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.4; // 转到 -100 到 100 y轴取反,并且放大洗漱
  
  // 取反按钮值，使得默认显示 0 false，按下显示 1 true
  int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
  int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
  // int convertedbuttonR1 = !remoteData.buttonR1; // 底部右
  // int convertedbuttonR2 = !remoteData.buttonR2; // 底部左

  // 跳跃
  if (convertedbuttonRB) { // 1=true，0=false
    wrobot.dir = 5; // 跳跃
  }else{
    wrobot.dir = 4; // 不跳跃
  }
  // Serial.print(convertedbuttonRB);

  // led蓝灯爆闪
  if (lastbuttonLB && !convertedbuttonLB) {
    blinkLed(); // 不能写在loop中，会阻断legloop，小车会倒下
  }
  // 更新上次 lastbuttonRB 的值
  lastbuttonLB = convertedbuttonLB;

  if(abs(convertedJ1PotY) > deadZone) {    // 上下高度 15 ~ 75
    wrobot.height = convertedJ1PotY;
  }else{
    wrobot.height = 0;
  }

  if(abs(convertedJ1PotX) > deadZone) {    // 左右摇摆 -30 ~ 30
    wrobot.roll  = convertedJ1PotX;
  }else{
    wrobot.roll = 0;
  }
  if(abs(convertedJ2PotY) > deadZone) {    // 前后运动 -100 到 100
    wrobot.joyy = convertedJ2PotY;
  }else{
    wrobot.joyy = 0;
  } 
  if(abs(convertedJ2PotX) > deadZone) { // 左右运动  -100 到 100
    wrobot.joyx = convertedJ2PotX; 
  } else {
    wrobot.joyx = 0;
  }
}

void setup()
{
  Serial.begin(115200);   // 通讯串口
  Serial2.begin(1000000); // 腿部sts舵机

  // Wifi初始化
  WiFi_SetAP(); 
  webserver.begin();
  webserver.on("/", HTTP_GET, basicWebCallback);
  websocket.begin();
  websocket.onEvent(webSocketEventCallback);

  // 初始化ESP-NOW
  WiFi.mode(WIFI_STA); // 打开这行注释就能开启sta模式，同时默认关闭ap热点模式，省电+稳定，而且不会影响espnow通信和整体代码框架
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  esp_now_register_recv_cb(OnDataRecv);

  // 舵机初始化
  sms_sts.pSerial = &Serial2;
  ID[0] = 1;
  ID[1] = 2;
  ACC[0] = 30;
  ACC[1] = 30;
  Speed[0] = 400;
  Speed[1] = 400;
  // Position[0] = 2048;
  // Position[1] = 2048;
  Position[0] = SERVO1_MIN;
  Position[1] = SERVO2_MIN;
  // 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
  sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);

  // 电压检测
  adc_calibration_init();
  adc1_config_width(width);
  adc1_config_channel_atten(channel, atten);
  esp_adc_cal_characterize(unit, atten, width, 0, &adc_chars);

  // 电量显示LED
  pinMode(LED_BAT, OUTPUT);

  // 编码器设置
  I2Cone.begin(19, 18, 400000UL);
  I2Ctwo.begin(23, 5, 400000UL);
  sensor1.init(&I2Cone);
  sensor2.init(&I2Ctwo);

  // mpu6050设置
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);

  // 连接motor对象与编码器对象
  motor1.linkSensor(&sensor1);
  motor2.linkSensor(&sensor2);

  // 速度环PID参数
  motor1.PID_velocity.P = 0.05;
  motor1.PID_velocity.I = 1;
  motor1.PID_velocity.D = 0;

  motor2.PID_velocity.P = 0.05;
  motor2.PID_velocity.I = 1;
  motor2.PID_velocity.D = 0;

  // 驱动器设置
  motor1.voltage_sensor_align = 6;
  motor2.voltage_sensor_align = 6;
  driver1.voltage_power_supply = 8;
  driver2.voltage_power_supply = 8;
  driver1.init();
  driver2.init();

  // 连接motor对象与驱动器对象
  motor1.linkDriver(&driver1);
  motor2.linkDriver(&driver2);

  motor1.torque_controller = TorqueControlType::voltage; // 扭矩控制器类型为 "电压模式"
  motor2.torque_controller = TorqueControlType::voltage;
  motor1.controller = MotionControlType::torque; // 运动控制器类型为 "扭矩模式"
  motor2.controller = MotionControlType::torque;
    

  // monitor相关设置
  motor1.useMonitoring(Serial);
  motor2.useMonitoring(Serial);
  // 电机初始化
  motor1.init();
  motor1.initFOC();
  motor2.init();
  motor2.initFOC();

  resetToDefault(); // 重置到默认参数状态
  delay(500);
}

void loop()
{
  
  bat_check();        // 电压检测
  web_loop();         // Web数据更新
  mpu6050.update();   // IMU数据更新
  lqr_balance_loop(); // lqr自平衡控制，更新LQR_u
  yaw_loop();         // yaw轴转向控制，更新YAW_output
  leg_loop();         // 腿部动作控制，控制舵机SyncWritePosEx

  // 记录上一次的遥控数据数据
  wrobot.dir_last = wrobot.dir;
  wrobot.joyx_last = wrobot.joyx;
  wrobot.joyy_last = wrobot.joyy;

  // 将自平衡计算输出转矩赋给电机
  // motor.target 负数小车向前转，正数小车向后转，
  motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取
  motor2.target = (-0.5) * (LQR_u - YAW_output);

  // 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
  //  Serial.println(LQR_angle);
  
  // 检测跳跃状态
  // if(jump_flag>0){
  //   Serial.println(jump_flag);
  // }
   

  // 检测小车是否向后马上要倒地了，在中间过程重置所有参数
  loopCounter++;
  // loopCounter作用：在刚开机的800次loop循环，不能进入resetToDefault()函数
  // resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直等于0，就躺地上不动了
  if ((LQR_angle < -35.0f || LQR_angle > 70.0f )&& loopCounter > 800 && resetToDefaultCounter < 130) {
      resetToDefault();
      resetToDefaultCounter++;
      motor1.target = 0;
      motor2.target = 0;
      // Serial.println("resetToDefault");
  }

  if( abs(LQR_angle) < 25.0f){
    resetToDefaultCounter = 0;
  }

  // target目标电压 0~8V, 0为不转, 8为最大转速
  // Serial.print("LQR_angle:");
  // Serial.print(LQR_angle);
  // Serial.print(",motor1.target:");
  // Serial.print(motor1.target);
  // Serial.print(",motor2.target:");
  // Serial.println(motor2.target);

  // 迭代计算FOC相电压
  motor1.loopFOC();
  motor2.loopFOC();

  // 设置轮部电机输出
  motor1.move();
  motor2.move();

  jump_loop(); // 跳跃控制

} // loop()

// lqr自平衡控制
void lqr_balance_loop()
{

  // LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得
  // angle_zeropoint，distance_zeropoint 角度零点和位置零点（在理想平衡状态下俯仰角度和位移目标值），用于和实际值做差，得到控制
  // lqr 控制参数： angle_control , gyro_control , distance_control , speed_control
  
  // LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试
  // LQR_u = LQR_k1*(LQR_angle - angle_zeropoint) 
        // + LQR_k2*LQR_gyro 
        // + LQR_k3*(LQR_distance - distance_zeropoint) 
        // + LQR_k4*LQR_speed;

  // 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转

  /*
  LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
  LQR_u = angle_control + gyro_control + distance_control + speed_control

  编码器反馈shaft_angle角度，shaft_velocity速度
  不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
  所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制

   angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关
                 = pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关
   gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关
                = pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关
    
    所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号
   distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关 
                    = pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint)  
                    //在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系
   
    speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度
                  //LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关
                  = pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy)) 
                  //LQR_u与实际shaft_velocity速度正相关
  */

  // 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5
  LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
  LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反
  LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，向前倒正数，向后倒负数
  LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度

  // 计算pitch的角度和角速度的pid控制值
  // 实际pitch角度与零点的差值，目标值是零
  // 计算角度误差，并通过 PID 控制器得到角度控制量
  angle_control = pid_angle(LQR_angle - angle_zeropoint); 
  // 实际pitch 角速度与目标值的差值，目标值是零
  // 通过 PID 控制器对角速度进行处理，得到角速度控制量
  gyro_control = pid_gyro(LQR_gyro);

  // 运动细节优化处理
  if (wrobot.joyy != 0) // 有前后方向运动指令时的处理
  {
    distance_zeropoint = LQR_distance; // 位移零点重置
    pid_lqr_u.error_prev = 0;          // 输出积分清零
  }
  // joyy=0，没有输出前后运动，但是小车可能被推动，运动指令复零时的原地停车处理
  if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) 
  {
    wrobot_move_stop_flag = 1;
  }
  if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5))
  {
    distance_zeropoint = LQR_distance; // 位移零点重置
    wrobot_move_stop_flag = 0;
  }

  if (abs(LQR_speed) > 15) // 被快速推动时的原地停车处理
  {
    distance_zeropoint = LQR_distance; // 位移零点重置
  }

  // 计算pitch的角度和角速度的pid控制值
  // 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零
  // 计算位移误差，并通过位移 PID 控制器得到位移控制量
  distance_control = pid_distance(LQR_distance - distance_zeropoint); 
  // 电机转速平均数(上面计算)-速度目标值=实际转速与目标值的差值, 目标值是零
  // 计算速度误差（实际速度减去经过低通滤波和缩放后的遥控器期望速度），并通过速度 PID 控制器得到速度控制量
  speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy));

  // 轮部离地检测, 连续记录两次的轮部转速
  robot_speed_last = robot_speed; // 记录连续两次的轮部转速
  robot_speed = LQR_speed;
  // 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理
  // 轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩
  if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0)) 
  {
    distance_zeropoint = LQR_distance;    // 位移零点重置
    LQR_u = angle_control + gyro_control;  // 位移和速度控制分量被忽略
    pid_lqr_u.error_prev = 0;             // 输出积分清零
  }
  else // 当轮部未离地时，LQR_u：4个参数
  {
    // 当轮部未离地时，LQR_u 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制
    LQR_u = angle_control + gyro_control + distance_control + speed_control;
    // 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行
    
  }

  // 小车没有控制的时候自稳定状态
  // 控制量lqr_u<5，控制量小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0
  if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0))
  {

    LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
    // Serial.println(LQR_u);

    // lpf_zeropoint 是一个低通滤波器，对 distance_control 进行滤波处理，减少高频噪声的影响。
    // 然后，将滤波后的 distance_control 作为输入，通过 pid_zeropoint 控制器进行进一步处理。
    // angle_zeropoint -= ...：将计算得到的调整量从 angle_zeropoint 中减去，实现重心自适应调整。
    // angle_zeropoint 是预设的角度零点，调整它可以让小车更好地适应不同的重心位置。
    angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应
  }
  else
  {
    pid_lqr_u.error_prev = 0; // 输出积分清零
  }

  // 平衡控制参数自适应
  if (wrobot.height < 50)
  {
    pid_speed.P = 0.7;
  }
  else if (wrobot.height < 64)
  {
    pid_speed.P = 0.6;
  }
  else
  {
    pid_speed.P = 0.5;
  }

}

// 腿部动作控制（舵机）
void leg_loop()
{

  // 倒地时强制腿部收缩
  if (LQR_angle < -30.0f ||  LQR_angle > 45.0f) 
  {
      Position[0] = SERVO1_MIN;
      Position[1] = SERVO2_MIN;
      sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
      return; // 跳过后续控制
  }

  if (jump_flag == 0) // 不处于跳跃状态
  {
    // 舵机加速度
    ACC[0] = 8;
    ACC[1] = 8;

    // 舵机速度
    Speed[0] = 300;
    Speed[1] = 300;
    float roll_angle = (float)mpu6050.getAngleX() + 2.0;
    leg_position_add = pid_roll_angle(lpf_roll(roll_angle));

    int roll_offset = wrobot.roll;
    int left_height, right_height;

    // Serial.print("roll_offset: ");
    // Serial.println(roll_offset);

    // 如果控制了 roll 角度，则调整左右脚的高度
    // 计算三角函数值
    float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度
    float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度)
    float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了
    if (roll_offset > 10) {
      // 正数，左脚高度 60，右脚高度 40
      // 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了
      left_height = wrobot.height + RollOffset + heightAdjust;
      right_height = wrobot.height + RollOffset - heightAdjust;
      leg_position_add = 0; // 不使用roll角度 pid计算
    } else if (roll_offset < -10) {
      // 负数，左脚高度 40，右脚高度 60
      left_height = wrobot.height + RollOffset - heightAdjust;
      right_height = wrobot.height + RollOffset + heightAdjust;
      leg_position_add = 0;
    } else { // 未控制 roll 角度
      // roll 为 0，左右恢复到同一高度
      left_height = wrobot.height;
      right_height = wrobot.height;
      // 没有人为控制roll角度，leg_position_add根据mpu6050 roll值 进行pid计算得到，以保持小车机身平稳
      // 但如果小车倒地，则不需要keep roll角度,否则小车倒在地上，1个脚长，1个脚短，很难看
      if (abs(LQR_angle) > 25.0f) 
      {
        leg_position_add = 0;
      }
    }

    // 根据新的高度计算舵机位置
    // id 1 [0] 正面看右手，id 2 正面看左手 [1]
    // 舵机id 1：1900（机身最矮）  2400（机身最高）2400-1900=500
    // 舵机id 2：2300（机身最矮）  1800（机身最高）2300-1800=500
    // 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32
    // const int LEG_HEIGHT_BASE = 32; // 这值越小，机身越高，实测范围 0（最高）-52（最低）
    Position[0] = 2048 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
    Position[1] = 2048 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;

    // 限制舵机位置范围
    if (Position[0] < SERVO1_MIN) // 舵机1最低
      Position[0] = SERVO1_MIN;
    else if (Position[0] > SERVO1_MAX) // 舵机1最高
      Position[0] = SERVO1_MAX;

    if (Position[1] > SERVO2_MIN) // 舵机2最低（和舵机1方向是反的）
      Position[1] = SERVO2_MIN;
    else if (Position[1] < SERVO2_MAX) // 舵机2最高
      Position[1] = SERVO2_MAX;
    
    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
  }
}

// 跳跃控制
void jump_loop()
{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次
  // 跳跃，腿部以最快速度伸到最长
  if ((wrobot.dir == 5) && (jump_flag == 0))
  { 

    // 跳跃开始
    ACC[0] = 0;
    ACC[1] = 0;
    Speed[0] = 0;
    Speed[1] = 0;
    Position[0] = SERVO1_MAX; 
    Position[1] = SERVO2_MAX;
    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);

    jump_flag = 1;

  }
  if (jump_flag > 0) // 跳跃中，腿部缩起来，等待30帧，对应上面设置 jump_flag = 1;
  {
    jump_flag++;

    if ((jump_flag > 30) && (jump_flag < 35))
    {
      ACC[0] = 0;
      ACC[1] = 0;
      Speed[0] = 0;
      Speed[1] = 0;
      // Position[0] = 2048 + 12 + 8.4 * (35 - 32); // 降低腿部高度，即车重心
      // Position[1] = 2048 - 12 - 8.4 * (35 - 32); 
      Position[0] = SERVO1_MIN + 150; // 收缩腿
      Position[1] = SERVO2_MIN - 150;
      sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);

      jump_flag = 40;
    }
    if (jump_flag > 200)
    {
      jump_flag = 0; // 跳跃过程结束
    }
  }
}

// yaw轴转向控制 （转向控制）
void yaw_loop()
{
  // YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro);
  yaw_angle_addup();

  YAW_angle_total += wrobot.joyx * 0.01; // 0.002
  float yaw_angle_control = pid_yaw_angle(YAW_angle_total);
  float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
  //  yaw 轴的角度和角速度数据进行了 PID 控制，得到 yaw_angle_control 和 yaw_gyro_control。
  // 然后，将这两个控制量相加，得到最终的 YAW_output。
  YAW_output = yaw_angle_control + yaw_gyro_control;
}

// Web数据更新
void web_loop()
{
  webserver.handleClient();
  websocket.loop();
  rp.spinOnce(); // 更新web端回传的控制信息
}

// yaw轴角度累加函数
void yaw_angle_addup()
{
  YAW_angle = (float)mpu6050.getAngleZ();
  YAW_gyro = (float)mpu6050.getGyroZ();

  if (YAW_angle_zero_point == (-10))
  {
    YAW_angle_zero_point = YAW_angle;
  }

  float yaw_angle_1, yaw_angle_2, yaw_addup_angle;
  if (YAW_angle > YAW_angle_last)
  {
    yaw_angle_1 = YAW_angle - YAW_angle_last;
    yaw_angle_2 = YAW_angle - YAW_angle_last - 2 * PI;
  }
  else
  {
    yaw_angle_1 = YAW_angle - YAW_angle_last;
    yaw_angle_2 = YAW_angle - YAW_angle_last + 2 * PI;
  }

  if (abs(yaw_angle_1) > abs(yaw_angle_2))
  {
    yaw_addup_angle = yaw_angle_2;
  }
  else
  {
    yaw_addup_angle = yaw_angle_1;
  }

  YAW_angle_total = YAW_angle_total + yaw_addup_angle;
  YAW_angle_last = YAW_angle;
}

void basicWebCallback(void)
{
  webserver.send(300, "text/html", basic_web);
}

void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
{
  if (type == WStype_TEXT)
  {
    String payload_str = String((char *)payload);
    StaticJsonDocument<300> doc;
    DeserializationError error = deserializeJson(doc, payload_str);

    String mode_str = doc["mode"];
    if (mode_str == "basic")
    {
      rp.parseBasic(doc);
    }
  }
}

// 电压检测初始化
void adc_calibration_init()
{
  if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK)
  {
    printf("eFuse Two Point: Supported\n");
  }
  else
  {
    printf("eFuse Two Point: NOT supported\n");
  }
  // Check Vref is burned into eFuse
  if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK)
  {
    printf("eFuse Vref: Supported\n");
  }
  else
  {
    printf("eFuse Vref: NOT supported\n");
  }
}

// 电压检测
void bat_check()
{
  if (bat_check_num > 1000)
  {
    // 电压读取
    uint32_t sum = 0;
    sum = analogRead(BAT_PIN);
    uint32_t voltage = esp_adc_cal_raw_to_voltage(sum, &adc_chars);
    double battery = (voltage * 3.97) / 1000.0;

    // Serial.println(battery);
    // 电量显示
    if (battery > 7.8)
      digitalWrite(LED_BAT, HIGH);
    else
      digitalWrite(LED_BAT, LOW);

    bat_check_num = 0;
  }
  else
    bat_check_num++;
}

// LED闪烁函数(不要用for，会阻塞其他程序)
void blinkLed() {
  digitalWrite(LED_BAT, HIGH);
  delay(200);
  digitalWrite(LED_BAT, LOW);
  delay(200);
  digitalWrite(LED_BAT, HIGH);
  delay(200);
  digitalWrite(LED_BAT, LOW);
  delay(200);
}

// 重置到默认参数状态，用于自平衡起立
void resetToDefault() {
    // LQR自平衡控制器参数
    LQR_angle = 0;
    LQR_gyro = 0;
    angle_control = 0;
    gyro_control = 0;
    angle_zeropoint = 5;

    LQR_speed = 0;
    LQR_distance = 0;
    speed_control = 0;
    distance_control = 0;
    distance_zeropoint = 1;
    LQR_u = 0;
    
    // YAW轴控制数据
    YAW_gyro = 0;
    YAW_angle = 0;
    YAW_angle_last = 0;
    YAW_angle_total = 0;
    YAW_angle_zero_point = -10;
    YAW_output = 0;

    // 逻辑处理标志位
    robot_speed = 0;
    robot_speed_last = 0;
    wrobot_move_stop_flag = 0;
    jump_flag = 0;
    leg_position_add = 0;

    pid_lqr_u.error_prev = 0;

    

}